# -*- coding: utf-8 -*-
"""
Created on Sat Sep  7 11:01:43 2024

@author: LENOVO
"""

import matplotlib.pyplot as plt
import numpy as np
from sympy import *
from scipy.optimize import root, fsolve
import pandas as pd

def pitch(H):
    a=16*H  #m
    b=H/(2*np.pi)
    vh=1 #m/s
    L0=(341-27.5*2)/100
    Lb=(220-27.5*2)/100

    f1=lambda theta:((a-b*(theta)))-4.5
    fai=root(f1,43)
    fai=fai.x[0]
    x=fai
    t=(b*x-a)*np.sqrt((a-b*x)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*x)**2+b**2)+a-b*x)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))
    
    r=lambda theta:((a-b*(theta)))
    thetas=lambda t:(a-np.sqrt(a**2-2*b*vh*t))/b #猜测角度值
    def theta(t):
        f=lambda x:(b*x-a)*np.sqrt((a-b*x)**2+b**2)/(2*b)-0.5*b*np.log(np.sqrt((a-b*x)**2+b**2)+a-b*x)+a/(2*b)*np.sqrt(a**2+b**2)+0.5*b*np.log(a+np.sqrt(a**2+b**2))-t
        theta=root(f,thetas(t))
        theta=theta.x[0]
        return theta

    THETA=[]
    X=[]
    Y=[]
    THETA.append(theta(t))
    X.append(r(THETA[0])*np.cos(THETA[0]))
    Y.append(-r(THETA[0])*np.sin(THETA[0]))

    f2=lambda thetai:(r(THETA[0]))**2+(r(thetai))**2-L0**2-2*(r(THETA[0]))*(r(thetai))*np.cos(THETA[0]-thetai)
    thetai=root(f2,THETA[0]-0.5)
    thetai=thetai.x[0]

    THETA.append(thetai)
    X.append(r(THETA[1])*np.cos((THETA[1])))
    Y.append(-r(THETA[1])*np.sin((THETA[1])))
    
    for j in range(0,222,1):
        
        f=lambda thetai:(r(THETA[1+j]))**2+(r(thetai))**2-Lb**2-2*(r(THETA[1+j]))*(r(thetai))*np.cos(THETA[1+j]-thetai)
        thetai=root(f,THETA[1+j]-0.5)
        thetai=thetai.x[0]
        THETA.append(thetai)
        X.append(r(THETA[j+2])*np.cos((THETA[j+2])))
        Y.append(-r(THETA[j+2])*np.sin((THETA[j+2])))

    A=np.column_stack((X, Y, THETA))


    
   #Head
    xh0=27.5/100
    yh0=15/100
    P=A
    c=P[0,0]-P[1,0]
    k0=(P[0,1]-P[1,1])/(P[0,0]-P[1,0])
    a1=1/(np.sqrt(1+k0**2)) #cos
    a2=k0/(np.sqrt(1+k0**2)) #sin
    a1=c*a1/abs(c)
    a2=c*a2/abs(c)
    xh=xh0*a1-yh0*a2+r(theta(t))*np.cos(theta(t))
    yh=xh0*a2+yh0*a1-r(theta(t))*np.sin(theta(t))
    H=np.array([xh,yh])
    #Tail
    xt0=-((341-27.5)/100)
    yt0=15/100
    xt=xt0*a1-yt0*a2+r(theta(t))*np.cos(theta(t))
    yt=xt0*a2+yt0*a1-r(theta(t))*np.sin(theta(t))
    T=np.array([xt,yt])


    dh=[]
    for i in range(1,223):
        k=(P[i,1]-P[i+1,1])/(P[i,0]-P[i+1,0])
        distance=abs(k*(H[0]-P[i,0])-H[1]+P[i,1])/np.sqrt(k**2+1)
        dh.append(distance)
        
    dt=[]
    for i in range(2,223):
        k=(P[i,1]-P[i+1,1])/(P[i,0]-P[i+1,0])
        distance=abs(k*(T[0]-P[i,0])-T[1]+P[i,1])/np.sqrt(k**2+1)
        dt.append(distance)   
    
    distance1=min(dh)
    distance2=min(dt)
    distance=min(distance1,distance2)
    sub=distance-0.15
    #画图
    # Storage=A
    # x_coords=Storage[:,0]
    # y_coords=Storage[:,1]
    # plt.plot(x_coords, y_coords,lw=3,c="r", marker='o')
    # return sub
    return sub
# print(pitch(0.55))
    
f3=lambda h:pitch(h)
h=fsolve(f3,0.45)
print(h)
print(pitch(h))

# print(pitch(0.430579147648045))

# print(pitch(0.5324405855572574))
# print(pitch(0.40522117))
# Hmin=[]
# for h in np.linspace(0.40,0.55,1000):
#     Hmin.append(pitch(h))
# print(Hmin)

# PITCH=[]    
# for h in np.linspace(0.40,0.55,1000):
#     PITCH.append(pitch(h))
# h=np.linspace(0.40,0.55,1000)
# PITCH=np.array(PITCH)
# plt.plot(h,PITCH,lw=3,marker="x",c='r')


